#include "Process.h"
#include "GlobalData.h"
#include "tcp_echoclient.h"
#include "lwip/tcp.h"
#include  "Sick_V2.h"
#include "EthData_Check.h"
#include <stdarg.h>
#include "commModule/protocol.h"
#include "commModule/protocol_upmotor.h"
#include "commModule/protocol_grapmotor.h"
#include "commModule/protocol_bms.h"
#include "commModule/protocol_bms.h"
#include "commModule/protocol_camera.h"
#include "CAN.h"
#include "GPIO.h"
#include "commData.h"
#include "Task.h"
#include "motor.h"
#include "Test.h"
#include "PS.h"
#include "usart.h"


uint8_t Sys_Heart_Tick(void);
uint8_t SickMgt_Task(void);
void usart1Printf(char *fmt,...);
uint8_t grapmotor_lowtx_new(uint8_t *buffer, uint8_t len);
uint8_t upmotor_lowtx_new(uint8_t *buffer, uint8_t len);
uint8_t bms_lowtx_new(uint8_t *buffer, uint8_t len);
uint8_t camera_lowtx_new(uint8_t *buffer, uint8_t len);
uint8_t upmotor_firstPosition_Set(void);
uint8_t Ingrap_Action_grap(void);
uint8_t Ingrap_Action_grap_v2(void);

extern struct tcp_pcb *echoclient_pcb;
extern struct echoclient *global_es;


uint8_t Test_Data[40] = {
0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0x00,
0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0x00,
0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0x00,
0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0x00};
uint8_t SingleReq_Data[17] = {
	0x02,0x73,0x52,0x4E,0x20,0x4C,0x4D,
	0x44,0x73,0x63,0x61,0x6E,0x64,0x61,
	0x74,0x61,0x03 
};
uint8_t ContinueReq_Start[19] = {
  0x02,0x73,0x45,0x4E,0x20,0x4C,0x4D,
	0x44,0x73,0x63,0x61,0x6E,0x64,0x61,
	0x74,0x61,0x20,0x31,0x03
};
uint8_t ContinueReq_Stop[20] = {
	0x02,0x73,0x45,0x4E,0x20,0x4C,0x4D,
	0x44,0x73,0x63,0x61,0x6E,0x64,0x61,
	0x74,0x61,0x20,0x30,0x03,0x0A
};

int main(void)
{
	static uint8_t size_last = 0;
	uint8_t len, i,firstPos_Volt;
    
	//note, timer is init in the MCUInit, PERIOD = 5ms
	MCUInit();
	//Sick Mgt pro	

    //for(;;);
    
	Sick_Circle_Init();
	upmotor_init(upmotor_lowtx_new);
	grapmotor_init(grapmotor_lowtx_new);
	//bms_init(bms_lowtx_new);
	//camera_init(camera_lowtx_new);

    TimerTask_Init();
	//note, the following 2 tasks must register
	//TimerTask_Register(_delay_ms, 1);	
    //TimerTask_Register(Sys_Heart_Tick, 500);	
	//TimerTask_Register(grapmotor_interfaceTest, 500);
	//TimerTask_Register(grapmotor_task, 20);
	//TimerTask_Register(upmotor_interfaceTest, 500);
	//TimerTask_Register(upmotor_task, 20);
    
#if 0 /* GrapMotor and UpperMotor test */
    for(;;){
        //Init position
        /*upmotor setting,first position find*/
        if(0!=upmotor_firstPosition_Set()){
            while(1){
                wait_for_timeout(500);
                firstPos_Volt = GPIO_ReadInputDataBit(JOY_A_KEY_Port,JOY_A_KEY_Pin);
                usart1Printf("upmotor position set error, please reset~%d", firstPos_Volt);
            }
        }
        usart1Printf("upmotor position set ok, please reset~");
        wait_for_timeout(100);
        
        if(1==Ingrap_Action_down(320)){
            for(;;){
                wait_for_timeout(100);
                usart1Printf("upmotor down set ok, please reset~");
            }
        }
        /*grapmotor setting, first position find*/
        usart1Printf("grapmotor first position find~");
        wait_for_timeout(50);
        if(0!=grapmotor_firstPosition_Set()){
            //while(1){
                wait_for_timeout(50);
                firstPos_Volt = GPIO_ReadInputDataBit(JOY_A_KEY_Port,JOY_A_KEY_Pin);
                usart1Printf("grapmotor position set error, please reset%d~",firstPos_Volt);
            //}
        }
        wait_for_timeout(50);
        usart1Printf("grapmotor first position set ok, please reset~");
        wait_for_timeout(50);
        usart1Printf("grapmotor grap set, please wait~");
        if(1==Ingrap_Action_grap_v2()){
            //while(1){
                wait_for_timeout(100);
                firstPos_Volt = GPIO_ReadInputDataBit(JOY_A_KEY_Port,JOY_A_KEY_Pin);
                usart1Printf("grapmotor grap set error, please reset%d~",firstPos_Volt);
            //}
        }
        usart1Printf("grapmotor grap set ok~");
        wait_for_timeout(100);
        usart1Printf("upmotor up set, please wait~");
        if(1==Ingrap_Action_Up(300)){
            for(;;){
                wait_for_timeout(100);
                usart1Printf("upmotor up set ok, please reset~");
            }
        }
        usart1Printf("loop test end~");
    }
    for(;;);
#endif   /* GrapMotor and UpperMotor test */
//	wait_for_timeout(500);
//	for(;;)
//	{
//		usart1Printf("grapmotor grap test begin~");
//		wait_for_timeout(100);
//		Ingrap_Action_grap_v2();
//		wait_for_timeout(100);
//		usart1Printf("grapmotor grap large test begin~");
//		if(0!=grapmotor_firstPosition_Set()){
//			usart1Printf("grapmotor position set error, please reset~");
//			while(1){
//				wait_for_timeout(500);
//				firstPos_Volt = GPIO_ReadInputDataBit(JOY_A_KEY_Port,JOY_A_KEY_Pin);
//				usart1Printf("grapmotor position set error, please reset%d~",firstPos_Volt);
//			}
//		}
//		wait_for_timeout(100);
//		usart1Printf("grapmotor grap test end~");
//		wait_for_timeout(100);
//	}
	//note, the period is related to the timer period
	TimerTask_Register(SickMgt_Task, 20);
	//TimerTask_Register(grapmotor_interfaceTest, 500);
	//TimerTask_Register(grapmotor_task, 10);
	//TimerTask_Register(bms_interfaceTest, 500);
	//TimerTask_Register(bms_task, 100);
	//TimerTask_Register(camera_interfaceTest, 100);
	//TimerTask_Register(camera_task, 20);
	TimerTask_Register(RunInModeTask, 5); 
    TimerTask_Register(MainInfoSendTask, 6); 
    //TimerTask_Register(MotorDriverTask, 13);
    //TimerTask_Register(OutPut_Data, 15);
    //TimerTask_Register(GetFlowerPosi, 4);
	/* Infinite loop */
    
//    leftMotorSpeed.rpm = 0;
//    rightMotorSpeed.rpm = 0;
//    leftMotorSpeed.dir = 1;
//    rightMotorSpeed.dir = 1;
//    MotorDriverTask();
//    wait_for_timeout(3000);

    rightMotorSpeed.rpm = 0;
    leftMotorSpeed.rpm = 0;
    MotorDriverTask();
    //wait_for_timeout(2000);
    while (1)
	{
        //PS_Scan();
		TimerTaskProcess();
		Test_CMD();
		ETHProcess();
		if(EthQueue.size>size_last){
			//pos = EthQueue.tail;
			Eth_RxComplete = 1;			
		}
		if(EthQueue.size>0){
			Sick_Result.IsSet = 0;
			Data_Input.IsCheck = 0;
			Sick_Circle_Mgt();
			len = Sick_Result.size;
//			for(i=0; i<len; i++)
//			{
//				usart1Printf("i->:%d, x->:%d, y->:%d, r2(cm^2)->:%d \n", 
//										 i, Sick_Result.Queue[i].circ_x, Sick_Result.Queue[i].circ_y, Sick_Result.Queue[i].circ_r2);
//			}
			Eth_ProComplete = 1;
			size_last = EthQueue.size;
		}
		//note, the data is Rx in the Process, Mgt in the Sick_Circle_Mgt		
	}
}

uint8_t Sys_Heart_Tick(void)
{
	usart1Printf("Loop heart \n");
	return 0;
}

uint8_t SickMgt_Task(void)
{
	static uint8_t Moni_Timer = 0;
	static uint8_t Wait_Timer = 0;
	if(ES_CONNECTED!=global_es->state){
	  if(Moni_Timer>50){
		  Moni_Timer = 0;
			//re-connect
			tcp_echoclient_connect();
		}else{
		  Moni_Timer++;
			return 1;
		}
	}else{
	  Moni_Timer = 0;
		if(0==Eth_RxComplete){
		  Wait_Timer++;
			if(Wait_Timer>5){
				Wait_Timer = 0;
				Buffer_Index = 0;
				//set wait flag
				Eth_RxComplete = 0;				
				tcp_write(echoclient_pcb,SingleReq_Data,17,TCP_WRITE_FLAG_COPY);
				tcp_output(echoclient_pcb);
			}
		}else{
		  if(1==Eth_ProComplete){
				Eth_ProComplete = 0;
				Wait_Timer = 0;
				Buffer_Index = 0;
				//set wait flag
				Eth_RxComplete = 0;
				tcp_write(echoclient_pcb,SingleReq_Data,17,TCP_WRITE_FLAG_COPY);
				tcp_output(echoclient_pcb);
			}else{
			  //wait pro
			}
			Wait_Timer = 0;
		}
	  return 0;
	}
	return 0;
}

#define	CMD_BUFFER_LEN	(100)
void usart1Printf(char *fmt,...)
{
    char buffer[CMD_BUFFER_LEN - 1];
    uint8_t i = 0;
    uint8_t len;

    va_list arg_ptr; //Define convert parameters variable
    va_start(arg_ptr, fmt); //Init variable
    len = vsnprintf(buffer, CMD_BUFFER_LEN+1, fmt, arg_ptr); //parameters list format to buffer
    
    while ((i < CMD_BUFFER_LEN) && (i < len) && (len > 0))
    {
      //LNRS485_SET_TX_Mode();
			//while (USART_GetFlagStatus(USART2, USART_FLAG_TC) == RESET);   
			//USART_SendData(USART2, (u8) buffer[i++]);
			//huart1.Instance->DR = (buffer[i++] & (uint8_t)0xFFU);
			while(0==(USART1->SR&0X40));
			USART1->DR = (uint8_t) buffer[i++];
			while(0==(USART1->SR&0X40));
      //while (USART_GetFlagStatus(USART2, USART_FLAG_TC) == RESET); 
			//LNRS485_SET_RX_Mode();
    }
    va_end(arg_ptr);
}
uint8_t camera_lowtx_new(uint8_t *buffer, uint8_t len)
{	
		uint8_t i = 0; 
		uint32_t id = 0;	
    while ((i < len) && (len > 0))
    {
			while(0==(USART1->SR&0X40));
			USART1->DR = (uint8_t) buffer[i++];
			while(0==(USART1->SR&0X40));
    }
		id = 0;
		for(i=0; i<4; i++){
		  id = (id<<8)+ buffer[len-1-i];
		}
		id = 0x100;
		Can1WriteData(id, 0, 1, len-4, buffer);
		//Can2WriteData(id, 0, 1, len-4, buffer);
		return 0;
}

uint8_t bms_lowtx_new(uint8_t *buffer, uint8_t len)
{	
		uint8_t i = 0;    
    while ((i < len) && (len > 0))
    {
			while(0==(USART1->SR&0X40));
			USART1->DR = (uint8_t) buffer[i++];
			while(0==(USART1->SR&0X40));
    }
		return 0;
}

uint8_t grapmotor_lowtx_new(uint8_t *buffer, uint8_t len)
{	
		uint8_t i = 0;    
    while ((i < len) && (len > 0))
    {
			//LNRS485_SET_TX_Mode();
			while(0==(USART1->SR&0X40));
			USART1->DR = (uint8_t) buffer[i++];
			//while(0==(USART1->SR&0X40));
			//LNRS485_SET_RX_Mode();
    }
		return 0;
}

uint8_t upmotor_lowtx_new(uint8_t *buffer, uint8_t len)
{	
		uint8_t i = 0;    
    while ((i < len) && (len > 0))
    {
		//LNRS485_SET_TX_Mode();
		//while (USART_GetFlagStatus(USART2, USART_FLAG_TC) == RESET);   
		//USART_SendData(USART2, (u8) buffer[i++]);
		//huart1.Instance->DR = (buffer[i++] & (uint8_t)0xFFU);
		while(0==(USART1->SR&0X40));
		USART1->DR = (uint8_t) buffer[i++];
		while(0==(USART1->SR&0X40));
		//while (USART_GetFlagStatus(USART2, USART_FLAG_TC) == RESET); 
		//LNRS485_SET_RX_Mode();
    }
		return 0;
}

uint8_t Pregrap_Action(void)
{
	int result = 0;
	result = upmotor_firstPosition_Set();
	if(1==result)
	{
		return 1;
	}
	result = grapmotor_firstPosition_Set();
	if(1==result)
	{
		return 2;
	}
	return 0;
}



